home *** CD-ROM | disk | FTP | other *** search
/ Ham Radio 2000 / Ham Radio 2000.iso / ham2000 / satellit / vstsrc / vstsgp4.c < prev    next >
C/C++ Source or Header  |  1995-01-21  |  22KB  |  623 lines
  1. /*
  2.  * %W% %E% %U%  [EXTREL_1.2]
  3.  *
  4.  * VersaTrack orbit calculations are based on those that appear in Dr. Manfred
  5.  * Bester's sattrack program (the Unix(tm) version, circa 1992).
  6.  *
  7.  * The data from which the maps where generated come from "xsat", an
  8.  * X-Windows program by David A. Curry (N9MSW).
  9.  *
  10.  * Site coordinates come from various sources, including a couple of
  11.  * World Almanacs, and also from both of the programs mentioned above.
  12.  *
  13.  * The following are authors' applicable copyright notices:
  14.  *
  15.  *                                                                               
  16.  * Copyright (c) 1992, 1993, 1994 Manfred Bester. All Rights Reserved.        
  17.  *                                                                           
  18.  * Permission to use, copy, modify, and distribute this software and its      
  19.  * documentation for educational, research and non-profit purposes, without   
  20.  * fee, and without a written agreement is hereby granted, provided that the  
  21.  * above copyright notice and the following three paragraphs appear in all    
  22.  * copies.                                                                    
  23.  *                                                                              
  24.  * Permission to incorporate this software into commercial products may be    
  25.  * obtained from the author, Dr. Manfred Bester, 1636 M. L. King Jr. Way,     
  26.  * Berkeley, CA 94709, USA.                                                   
  27.  *                                                                             
  28.  * IN NO EVENT SHALL THE AUTHOR BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT,  
  29.  * SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OF    
  30.  * THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE AUTHOR HAS BEEN ADVISED   
  31.  * OF THE POSSIBILITY OF SUCH DAMAGE.                                         
  32.  *                                                                             
  33.  * THE AUTHOR SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT       
  34.  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A    
  35.  * PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS"       
  36.  * BASIS, AND THE AUTHOR HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,  
  37.  * UPDATES, ENHANCEMENTS, OR MODIFICATIONS.                                   
  38.  *                                                                             
  39.  *                                                                             
  40.  * Copyright 1992 by David A. Curry                                            
  41.  *                                                                             
  42.  * Permission to use, copy, modify, distribute, and sell this software and its 
  43.  * documentation for any purpose is hereby granted without fee, provided that  
  44.  * the above copyright notice appear in all copies and that both that copyright
  45.  * notice and this permission notice appear in supporting documentation.  The  
  46.  * author makes no representations about the suitability of this software for  
  47.  * any purpose.  It is provided "as is" without express or implied warranty.   
  48.  *                                                                             
  49.  * David A. Curry, N9MSW                                                       
  50.  * Purdue University                                                           
  51.  * Engineering Computer Network                                                
  52.  * 1285 Electrical Engineering Building                                        
  53.  * West Lafayette, IN 47907                                                    
  54.  * davy@ecn.purdue.edu                                                         
  55.  *                                                                             
  56.  * VersaTrack Copyright (c) 1993, 1994 Siamack Navabpour. All Rights Reserved.
  57.  *
  58.  * Permission is hereby granted to copy, modify and distribute VersaTrack
  59.  * in whole, or in part, for educational, non-profit and non-commercial use
  60.  * only, free of charge or obligation, and without agreement, provided that
  61.  * all copyrights and restrictions noted herein are observed and followed, and
  62.  * additionally, that this and all other copyright notices listed herein
  63.  * appear unaltered in all copies and in all derived work.
  64.  *
  65.  * This notice shall not in any way void or superceed any of the other authors
  66.  * rights or privilages.
  67.  *
  68.  * VersaTrack IS PRESENTED FREE AND "AS IS", WITHOUT ANY WARRANTY OR SUPPORT.
  69.  * YOU USE IT AT YOUR OWN RISK. The author(s) shall not be liable for any
  70.  * direct, indirect, incidental, or consequential damage, loss of profits or
  71.  * other tangible or intangible losses or benefits, arising out of or related
  72.  * to its use. VersaTrack carries no warranty, explicit or implied, including
  73.  * but not limited to those of merchantability and fitness for a particular
  74.  * purpose.
  75.  *
  76.  * Siamack Navabpour, 12342 Hunter's Chase Dr. Apt. 2114, Austin, TX 78729.
  77.  * sia@bga.com or sia@realtime.com.
  78.  */
  79.  
  80.  
  81. #include <windows.h>
  82. #include <stdio.h>
  83. #include <math.h>
  84.  
  85. #include "vstdefs.h"
  86. #include "constant.h"
  87. #include "vsttype.h"
  88. #include "vstextrn.h"
  89. #include "vstprop.h"
  90.  
  91. /*
  92.  * changeNoradUnits: changes units of fundamental elements
  93.  *      for use with NORAD model
  94.  */
  95.  
  96. static void
  97. changeNoradUnits(sp, p)
  98. select_t *sp;
  99. register satprop_t *p;
  100. {
  101.     register track_t *tp = sp->sl_tp;
  102.  
  103.     p->epoch0     = tp->epochday;
  104.     p->ecc0       = tp->eccentricity;
  105.  
  106.     p->inc0       = tp->inclination;                         /* [rad]         */
  107.     p->cosInc     = cos(tp->inclination);
  108.     p->sinInc     = sin(tp->inclination);
  109.  
  110.     p->meanMot0   = tp->epochmeanmotion * TWOPI / MPD;       /* [rad/min]     */
  111.     p->meanAn0    = tp->epochmeananomaly;                    /* [rad]         */
  112.     p->argPer0    = tp->epochargperigee;                     /* [rad]         */
  113.     p->raan0      = tp->epochRAAN;                           /* [rad]         */
  114.  
  115.     p->decRate    = tp->decayrate * TWOPI / MPD2;            /* [rad/min^2]   */
  116.     p->decRateDot = tp->satp->s_dratedot * TWOPI / MPD3;     /* [rad/min^3]   */
  117.     p->bStar      = tp->satp->s_bstarcoeff / AE;
  118. }
  119.  
  120. /*
  121.  * getSemiMajorAxis: recover original mean motion (meanMotDeep)
  122.  *        and semimajor axis (sma0Deep) from input elements
  123.  */
  124.  
  125. static void
  126. getSemiMajorAxis(sp, p)
  127. select_t *sp;
  128. register satprop_t *p;
  129. {    
  130.     double rDelta0, rDelta1, rDelta12, rDelta13, rEccSQ, rEccCB;
  131.     double rSma0, rSma1;
  132.     
  133.     changeNoradUnits(sp, p);
  134.  
  135.     p->cos2Inc       = SQR(p->cosInc);
  136.     p->cos4Inc       = SQR(p->cos2Inc);
  137.  
  138.     p->x3thm1        = 3.0 * p->cos2Inc - 1.0;
  139.     p->x7thm1        = 7.0 * p->cos2Inc - 1.0;
  140.     p->x1m1th        = 1.0 - p->cos2Inc;
  141.     p->x1m5th        = 1.0 - 5.0 * p->cos2Inc;
  142.  
  143.     rEccSQ           = SQR(p->ecc0);
  144.     rEccCB           = p->ecc0 * rEccSQ;
  145.  
  146.     p->beta02        = 1.0 - rEccSQ;                     /* needed elsewhere */
  147.     p->beta0         = sqrt(p->beta02);                  /* needed elsewhere */
  148.     p->beta03        = p->beta0 * p->beta02;             /* needed elsewhere */
  149.  
  150.     rSma1            = pow((KE/p->meanMot0), TWOTHIRDS);
  151.  
  152.     rDelta1          = 1.5 * CK2 * p->x3thm1 / (SQR(rSma1) * p->beta03);
  153.     rDelta12         = SQR(rDelta1);
  154.     rDelta13         = rDelta1 * rDelta12;
  155.  
  156.     rSma0            = rSma1 * (1.0 - ONETHIRD * rDelta1 - rDelta12 
  157.                                 - 134.0/81.0 * rDelta13);
  158.  
  159.     rDelta0          = 1.5 * CK2 * p->x3thm1 / (SQR(rSma0) * p->beta03);
  160.  
  161.     p->meanMotDeep   = p->meanMot0 / (1.0 + rDelta0);
  162.     p->sma0Deep      = rSma0 / (1.0 - rDelta0);
  163.     p->semiMajorAxis = p->sma0Deep * EARTHRADIUS;
  164.  
  165.     p->deepSpaceFlag = (TWOPI / p->meanMotDeep >= 225.0) ? TRUE : FALSE;
  166. }
  167.  
  168. /*
  169.  * getTimeArgs: gets time arguments for the SGP4/SDP4 model calculations
  170.  */
  171.  
  172. static void
  173. getTimeArgs(tmArg, p)
  174. double tmArg;
  175. register satprop_t *p;
  176. {
  177.     p->tFP = (tmArg - p->epoch0) * MPD;                            /* [min] */
  178.     p->tSQ = p->tFP * p->tFP;
  179.     p->tCB = p->tSQ * p->tFP;
  180.     p->tQD = p->tCB * p->tFP;
  181.     p->tQN = p->tQD * p->tFP;
  182. }
  183.  
  184. /*
  185.  * checkPerigeeHeight: checks perigee height and make modifications
  186.  *                     accordingly
  187.  */
  188.  
  189. static void
  190. checkPerigeeHeight(p)
  191. register satprop_t *p;
  192. {
  193.     p->perigeeHeight = (p->sma0Deep * (1.0 - p->ecc0) - AE) * EARTHRADIUS;
  194.  
  195.     p->truncFlag     = (p->perigeeHeight < 220.0) ? TRUE : FALSE;
  196.     p->satCrashFlag  = (p->perigeeHeight <   0.0) ? TRUE : FALSE;
  197.  
  198.     p->sStar = S;
  199.     p->qmst4 = Q0MS0T4;
  200.  
  201.     if (p->perigeeHeight < 156.0) {
  202.         p->sStar = p->perigeeHeight - S0;
  203.  
  204.         if (p->perigeeHeight <= 98.0)
  205.             p->sStar = 20.0;
  206.  
  207.         p->qmst4  = pow(((Q0 - p->sStar) * AE / EARTHRADIUS),4.0);
  208.         p->sStar /= EARTHRADIUS;
  209.         p->sStar += AE;
  210.     }
  211. }
  212.  
  213. /*
  214.  * calcCommonPerturb: calculates quantities that are common to the SGP4 and
  215.  *                    SDP4 models
  216.  */
  217.  
  218. static void
  219. calcCommonPerturb(p)
  220. register satprop_t *p;
  221. {
  222.     double rC2, rCoef1, rEtaSQ, rPInvSQ, rPsiSQ;
  223.     
  224.     rPInvSQ   = 1.0 / (SQR(p->sma0Deep) * SQR(p->beta02));
  225.     p->xi     = 1.0 / (p->sma0Deep - p->sStar);
  226.     p->eta    = p->sma0Deep * p->ecc0 * p->xi;
  227.     rEtaSQ    = SQR(p->eta);
  228.     p->eccEta = p->ecc0 * p->eta;
  229.     rPsiSQ    = fabs(1.0 - rEtaSQ);
  230.     p->coef0  = p->qmst4 * pow(p->xi, 4.0);
  231.     rCoef1    = p->coef0 / pow(rPsiSQ, 3.5);
  232.  
  233.     rC2       = rCoef1 * p->meanMotDeep * 
  234.                 (p->sma0Deep * (1.0 + 1.5 * rEtaSQ + p->eccEta *
  235.                 (4.0 + rEtaSQ)) + 0.75 * CK2 * p->xi / rPsiSQ * p->x3thm1 *
  236.                 (8.0 + 3.0 * rEtaSQ * (8.0 + rEtaSQ)));
  237.  
  238.     p->c1     = p->bStar * rC2;
  239.  
  240.     p->c3     = p->coef0 * p->xi * A30CK2 * p->meanMotDeep * AE *
  241.                 p->sinInc / p->ecc0;
  242.  
  243.     p->c4     = 2.0 * p->meanMotDeep * rCoef1 * p->sma0Deep * p->beta02 * 
  244.                 (p->eta * (2.0 + 0.5 * rEtaSQ) + p->ecc0 *
  245.                 (0.5 + 2.0 * rEtaSQ) - 2.0 * CK2 * p->xi /
  246.                 (p->sma0Deep * rPsiSQ) * (-3.0 * p->x3thm1 *
  247.                 (1.0 - 2.0 * p->eccEta + rEtaSQ * 
  248.                 (1.5 - 0.5 * p->eccEta)) + 0.75 * p->x1m1th * 
  249.                 (2.0 * rEtaSQ - p->eccEta * (1.0 + rEtaSQ))
  250.                 * cos(2.0 * p->argPer0)));
  251.  
  252.     p->c5     = 2.0 * rCoef1 * p->sma0Deep * p->beta02 * 
  253.                 (1.0 + 2.75 * (rEtaSQ + p->eccEta) + p->eccEta * rEtaSQ);
  254.  
  255.     p->clc1   = 3.0  * CK2 * rPInvSQ * p->meanMotDeep;
  256.     p->clc2   = p->clc1 * CK2 * rPInvSQ;
  257.     p->clc3   = 1.25 * CK4 * SQR(rPInvSQ) * p->meanMotDeep;
  258. }
  259.  
  260. /*
  261.  * updateGravityAndDrag: updates quantities for secular gravity and
  262.  *                       atmospheric drag
  263.  */
  264.  
  265. static void
  266. updateGravityAndDrag(p)
  267. register satprop_t *p;
  268. {
  269.     double rClc0, rDelA, rDelAM, rDelArgPer, rDelE, rDelMeanAn;
  270.     double rDelL, rD2, rD3, rD4, rT3Cof, rT4Cof, rT5Cof;
  271.     double rMeanAnP, rSinMeanAnP, rSinMeanAn0;
  272.     
  273.     p->meanAnDot = p->meanMotDeep + 0.5 * p->clc1 * p->beta0 * p->x3thm1 +
  274.                    0.0625 * p->clc2 * p->beta0 *
  275.                    (13.0 - 78.0 * p->cos2Inc + 137.0 * p->cos4Inc);
  276.  
  277.     p->argPerDot = -0.5 * p->clc1 * p->x1m5th + 0.0625 * p->clc2 * 
  278.                    (7.0 - 114.0 * p->cos2Inc + 395.0 * p->cos4Inc) + 
  279.                    p->clc3 * (3.0 - 36.0 * p->cos2Inc + 49.0 * p->cos4Inc);
  280.  
  281.     p->raanDot   = -p->clc1 * p->cosInc + (0.5 * p->clc2 * (4.0 - 19.0 *
  282.                    p->cos2Inc) + 2.0 * p->clc3 * (3.0 -  7.0 * p->cos2Inc)) *
  283.                    p->cosInc;
  284.  
  285.     p->meanAnDF  = p->meanAn0 + p->meanAnDot * p->tFP;
  286.     p->argPerDF  = p->argPer0 + p->argPerDot * p->tFP;
  287.     p->raanDF    = p->raan0   + p->raanDot   * p->tFP;
  288.  
  289.     rDelA        = 1.0 - p->c1 * p->tFP;
  290.     rDelE        = p->bStar * p->c4 * p->tFP;
  291.     rDelL        = 1.5 * p->c1 * p->tSQ;
  292.  
  293.     if (!p->truncFlag) {
  294.         p->c1SQ       = SQR(p->c1);
  295.         rD2           = 4.0 * p->sma0Deep * p->xi * p->c1SQ;
  296.         rClc0         = rD2 * p->xi * p->c1 / 3.0;
  297.         rD3           = (17.0 * p->sma0Deep + p->sStar) * rClc0;
  298.         rD4           = 0.5 * rClc0 * p->sma0Deep * p->xi * 
  299.                         (221.0 * p->sma0Deep + 31.0 * p->sStar) * p->c1;
  300.  
  301.         rT3Cof        = rD2 + 2.0 * p->c1SQ;
  302.         rT4Cof        = 0.25 * (3.0 * rD3 + p->c1 * (12.0 * rD2 + 10.0 *
  303.                         p->c1SQ));
  304.         rT5Cof        = 0.2 * (3.0 * rD4 + 12.0 * p->c1 * rD3 + 
  305.                         6.0 * SQR(rD2) + 15.0 * p->c1SQ *
  306.                         (2.0 * rD2 + p->c1SQ));
  307.  
  308.         p->argPerCof  = p->bStar * p->c3 * cos(p->argPer0);
  309.         p->meanAnCof  = -TWOTHIRDS * p->coef0 * p->bStar * AE / p->eccEta;
  310.         p->raanCof    = -3.5 * p->beta02 * p->clc1 * p->cosInc * p->c1;
  311.  
  312.         rDelArgPer    = p->argPerCof * p->tFP;
  313.         rDelMeanAn    = p->meanAnCof *
  314.                         ( pow(1.0 + p->eta * cos(p->meanAnDF),3.0) - 
  315.                           pow(1.0 + p->eta * cos(p->meanAn0),3.0) );
  316.  
  317.         rDelAM        = rDelArgPer + rDelMeanAn;
  318.         rMeanAnP      = p->meanAnDF + rDelAM;
  319.         p->argPer     = p->argPerDF - rDelAM;
  320.  
  321.         rSinMeanAn0   = sin(p->meanAn0);
  322.         rSinMeanAnP   = sin(rMeanAnP);
  323.  
  324.         rDelA        -= rD2 * p->tSQ - rD3 * p->tCB - rD4 * p->tQD;
  325.         rDelE        += p->bStar * p->c5  *
  326.                         (rSinMeanAnP - rSinMeanAn0);
  327.         rDelL        += rT3Cof * p->tCB + rT4Cof * p->tQD +
  328.                         rT5Cof * p->tQN;
  329.     }
  330.     else {
  331.         rMeanAnP    = p->meanAnDF;
  332.         p->argPer     = p->argPerDF;
  333.     }
  334.  
  335.     p->raan    = p->raanDF - p->raanCof * p->tSQ;
  336.  
  337.     p->smallE  = p->ecc0 - rDelE;
  338.     p->smallA  = p->sma0Deep * SQR(rDelA);
  339.  
  340.     p->xL      = rMeanAnP + p->argPer + p->raan + p->meanMotDeep * rDelL;
  341.     p->beta    = sqrt(1.0 - SQR(p->smallE));
  342.     p->meanMot = KE / pow(p->smallA, 1.5);
  343.  
  344.     p->perigeeHeight  = (p->smallA * (1.0 - p->ecc0) - AE) * EARTHRADIUS; /*km*/
  345.  
  346.     p->curArgPerigeeX = p->argPer;                              /* [rad]   */
  347.     p->curRaanX       = p->raan;                                /* [rad]   */
  348.     p->curMotionX     = p->meanMot * MPD / TWOPI;               /* [rev/d] */
  349. }
  350.  
  351. /*
  352.  * updateLongPeridics: updates long periodic quantities
  353.  */
  354.  
  355. static void
  356. updateLongPeriodics(p)
  357. register satprop_t *p;
  358. {
  359.     double rUpd6, rAyCof, rXLCoef, rXLL;
  360.     
  361.     rUpd6    = 1.0 / (p->smallA * SQR(p->beta));
  362.  
  363.     p->axN   = p->smallE * cos(p->argPer);
  364.  
  365.     rXLCoef  = 0.125 * A30CK2 * p->sinInc * (3.0 + 5.0 * p->cosInc) /
  366.                (1.0 + p->cosInc);
  367.     rXLL     = rUpd6 * rXLCoef * p->axN;
  368.     p->xLT   = p->xL + rXLL;
  369.  
  370.     rAyCof   = 0.250 * A30CK2 * p->sinInc;
  371.     p->ayNL  = rUpd6 * rAyCof;
  372.     p->ayN   = p->smallE * sin(p->argPer) + p->ayNL;
  373. }
  374.  
  375. /*
  376.  * updateShortPeridics: gets osculating quantities by adding delta terms
  377.  */
  378.  
  379. static void
  380. updateShortPeriodics(p)
  381. register satprop_t *p;
  382. {
  383.     double rCosU, rCos2U, rDelInc, rDelR, rDelRDot, rDelRFDot, rDelRaan, rDelU;
  384.     double rESinE, rECosE, rEL2, rPL, rSinU, rSin2U, rSmallR, rSmallU;
  385.     double rUpd1, rUpd2, rUpd3, rUpd4, rUpd5;
  386.     
  387.     rECosE      = p->kep4 + p->kep5;
  388.     rESinE      = p->kep2 - p->kep3;
  389.  
  390.     rEL2        = SQR(p->axN) + SQR(p->ayN);
  391.     rUpd1       = 1.0 - rEL2;
  392.     rPL         = p->smallA * rUpd1;
  393.     p->betaL    = sqrt(rUpd1);
  394.  
  395.     rSmallR     = p->smallA * (1.0 - rECosE);
  396.  
  397.     p->rDot     = KE * sqrt(p->smallA) / rSmallR * rESinE;
  398.     p->rfDot    = KE * sqrt(rPL) / rSmallR;
  399.  
  400.     rUpd2       = p->smallA / rSmallR;
  401.     rUpd3       = 1.0 / (1.0 + p->betaL);
  402.  
  403.     rCosU       = rUpd2 * (p->cosEpAP - p->axN + p->ayN * rESinE * rUpd3);
  404.     rSinU       = rUpd2 * (p->sinEpAP - p->ayN - p->axN * rESinE * rUpd3);
  405.  
  406.     rSin2U      = 2.0 * rSinU * rCosU;
  407.     rCos2U      = 2.0 * rCosU * rCosU - 1.0;
  408.  
  409.     rSmallU     = atan2(rSinU, rCosU);
  410.     rSmallU     = reduce(rSmallU,ZERO,TWOPI);
  411.  
  412.     rUpd4       = CK2 / SQR(rPL);
  413.     rUpd5       = CK2 / rPL;
  414.  
  415.     rDelR       = 0.5 * rUpd5 * p->x1m1th * rCos2U;
  416.     rDelU       = -0.25 * rUpd4 * p->x7thm1 * rSin2U;
  417.  
  418.     rDelRaan    = 1.5 * rUpd4 * p->cosInc * rSin2U;
  419.     rDelInc     = 1.5 * rUpd4 * p->cosInc * p->sinInc * rCos2U;
  420.  
  421.     rDelRDot    = -p->meanMot * rUpd5 * p->x1m1th * rSin2U;
  422.     rDelRFDot   = p->meanMot * rUpd5 * (p->x1m1th * rCos2U + 1.5
  423.                   * p->x3thm1);
  424.  
  425.     p->rk       = rSmallR * (1.0 - 1.5 * rUpd4 * p->betaL * p->x3thm1) +
  426.                   rDelR;
  427.  
  428.     p->uk       = rSmallU + rDelU;
  429.     p->raanK    = p->raan   + rDelRaan;
  430.     p->incK     = p->inc0   + rDelInc;
  431.     p->rkDot    = p->rDot   + rDelRDot;
  432.     p->rfkDot   = p->rfDot  + rDelRFDot;
  433.  
  434.     p->curArgNodeX = p->uk;
  435. }
  436.  
  437. /*
  438.  * keplerX: solve Kepler's equation
  439.  */
  440.  
  441. static void
  442. keplerX(p)
  443. register satprop_t *p;
  444. {
  445.     double rCapU, rCapEpAP, rSinEpAP, rCosEpAP, diffAnom, eccAnom;
  446.     double rKep1, rKep2, rKep3, rKep4, rKep5;
  447.  
  448.     rCapU    = reduce(p->xLT - p->raan, ZERO, TWOPI);
  449.     rCapEpAP = rCapU;
  450.  
  451.     do {
  452.         rKep1    = rCapEpAP;
  453.         rSinEpAP = sin(rKep1);
  454.         rCosEpAP = cos(rKep1);
  455.  
  456.         rKep2    = p->axN * rSinEpAP;
  457.         rKep3    = p->ayN * rCosEpAP;
  458.         rKep4    = p->axN * rCosEpAP;
  459.         rKep5    = p->ayN * rSinEpAP;
  460.  
  461.         rCapEpAP = rKep1 + (rCapU - rKep1 + rKep2 - rKep3) / 
  462.                            (1.0 - rKep4 - rKep5);
  463.  
  464.         diffAnom = rCapEpAP - rKep1;
  465.  
  466.     } while (fabs(diffAnom) >= ONEPPM);               /* 0.2 arcsec precision */
  467.  
  468.     p->sinEpAP = rSinEpAP;
  469.     p->cosEpAP = rCosEpAP;
  470. #if 0
  471.     p->kep1    = rKep1;
  472. #endif
  473.     p->kep2    = rKep2;
  474.     p->kep3    = rKep3;
  475.     p->kep4    = rKep4;
  476.     p->kep5    = rKep5;
  477.  
  478.     eccAnom = rCapEpAP - p->argPer;
  479.  
  480.     if (fabs(eccAnom) < ONEPPM)
  481.         p->trueAnomalyX = PI;
  482.     else
  483.         p->trueAnomalyX = 2.0 * atan(sqrt((1.0 + p->ecc0) /
  484.                             (1.0 - p->ecc0)) * tan(eccAnom / 2.0));
  485.  
  486.     p->trueAnomalyX = reduce(p->trueAnomalyX, ZERO, TWOPI);
  487. }
  488.  
  489. #if 0
  490. /*
  491.  * getOrbitNumberX: gets orbit number and mean anomaly
  492.  */
  493.  
  494. static void
  495. getOrbitNumberX(tArg, sp, p)
  496. double tArg;
  497. select_t *sp;
  498. satprop_t *p;
  499. {
  500.     double dT, of, curOrbit, fdummy;
  501.     track_t *tp = sp->sl_tp;
  502.  
  503.     dT             = tArg - p->epoch0;
  504.  
  505.     tp->reforbit   = (double) tp->epochorbitnum + tp->epochmeananomaly
  506.                      / TWOPI;                                       /* [rev] */
  507.     curOrbit       = tp->reforbit + p->curMotionX * dT -
  508.                      (p->curMotionX - tp->epochmeanmotion) * dT;
  509.  
  510.     tp->orbitnum   = (int) curOrbit + 1L;
  511.     of             = modf(curOrbit, &fdummy);
  512.     tp->orbitfrac  = (int) (of * 100.0);
  513.     tp->meananomaly= of * TWOPI;
  514. }
  515.  
  516. #endif
  517.  
  518.  
  519. /*
  520.  * getStateVectorX: calculates position and velocity components
  521.  */
  522.  
  523. static void
  524. satposX(sp, p, ttime, X, Y, Z, Radius, vX, vY, vZ)
  525. select_t *sp;
  526. satprop_t *p;
  527. double ttime, *X, *Y, *Z, *Radius, *vX, *vY, *vZ;
  528. {
  529.     double mVec[3], nVec[3], uVec[3], vVec[3], satVelS[3], satPosS[3];
  530.     double cosIK, cosRK, cosUK, sinIK, sinRK, sinUK;
  531.     double dT, curOrbit, fdummy, of;
  532.     track_t *tp = sp->sl_tp;
  533.     int    i;
  534.     extern SquintAngle(select_t *, double, double, double, double);
  535.  
  536.     cosUK   = cos(p->uk);
  537.     sinUK   = sin(p->uk);
  538.     cosIK   = cos(p->incK);
  539.     sinIK   = sin(p->incK);
  540.     cosRK   = cos(p->raanK);
  541.     sinRK   = sin(p->raanK);
  542.  
  543.     mVec[0] = -sinRK * cosIK;
  544.     mVec[1] = cosRK * cosIK;
  545.     mVec[2] = sinIK;
  546.  
  547.     nVec[0] = cosRK;
  548.     nVec[1] = sinRK;
  549.     nVec[2] = 0.0;
  550.  
  551.     for (i = 0; i <= 2; i++) {
  552.         uVec[i]     = mVec[i] * sinUK + nVec[i] * cosUK;
  553.         vVec[i]     = mVec[i] * cosUK - nVec[i] * sinUK;
  554.         satPosS[i]  = (p->rk * uVec[i]) * EARTHRADIUS;
  555.         satVelS[i]  = (p->rkDot * uVec[i] + p->rfkDot * vVec[i]) * (EARTHRADIUS / 60.0);
  556.     }
  557.  
  558.     *X = satPosS[0];
  559.     *Y = satPosS[1];
  560.     *Z = satPosS[2];
  561.  
  562.     *vX = satVelS[0];
  563.     *vY = satVelS[1];
  564.     *vZ = satVelS[2];
  565.  
  566.     dT              = ttime - p->epoch0;
  567.     tp->reforbit    = (double) tp->epochorbitnum + tp->epochmeananomaly
  568.                       / TWOPI;                                       /* [rev] */
  569.  
  570.     curOrbit        = tp->reforbit + p->curMotionX * dT -
  571.                       (p->curMotionX - tp->epochmeanmotion) * dT;
  572.  
  573.     tp->orbitnum    = (int) curOrbit + 1L;
  574.     of              = modf(curOrbit, &fdummy);
  575.     tp->orbitfrac   = (int) (of * 100.0);
  576.     tp->meananomaly = of * TWOPI;
  577.     tp->trueanomaly = p->trueAnomalyX;
  578.  
  579.     *Radius = tp->semimajoraxis * (1.0 - SQR(tp->eccentricity))
  580.               / (1.0 + (tp->eccentricity * cos(tp->trueanomaly)));
  581.  
  582.     SquintAngle(sp, tp->trueanomaly, p->curRaanX, p->curArgPerigeeX, *Radius);
  583. }
  584.  
  585. /*
  586.  * calcSGP4: calculates SGP4 propagation model
  587.  *
  588.  * For perigee less than 220 km, the truncFlag is set and the equations for
  589.  * 'smallA' and 'xL' are truncated after the 'c1' term, and the terms
  590.  *  involving 'c5', 'deltaArgPer' and 'deltaMeanAn' are dropped.
  591.  *
  592.  * For a perigee below 156 km, the values of S and Q0MS0T4 are altered.
  593.  * (see 'checkPerigeeHeight()')
  594.  *
  595.  */
  596.  
  597. void
  598. satposSGP4(sp, ttime, X, Y, Z, Radius, vX, vY, vZ)
  599. select_t *sp;
  600. double ttime, *X, *Y, *Z, *Radius, *vX, *vY, *vZ;
  601. {
  602.     /*
  603.      * HUEMONGUS STRUCT WARNING - BEWARE OF STACK OVERFLOW!
  604.      */
  605.     satprop_t prop_params;
  606.  
  607.     memset(&prop_params, 0, sizeof(prop_params));
  608.     getSemiMajorAxis(sp, &prop_params);
  609.         
  610.     checkPerigeeHeight(&prop_params);
  611.  
  612.     if (prop_params.satCrashFlag)
  613.         return;
  614.  
  615.     getTimeArgs(ttime, &prop_params);
  616.     calcCommonPerturb(&prop_params);
  617.     updateGravityAndDrag(&prop_params);
  618.     updateLongPeriodics(&prop_params);
  619.     keplerX(&prop_params);
  620.     updateShortPeriodics(&prop_params);
  621.     satposX(sp, &prop_params, ttime, X, Y, Z, Radius, vX, vY, vZ);
  622. }
  623.